import numpy as np
import cv2  # 用于视频保存
import os
from  build_local_lidar_map import build_local_lidar_map, build_local_lidar_update_map
from  parse_data import load_lidar_data_parse_rpy
from  draw_map import visualize_line
from  grid_map import BuildGridMap, updateMap

# 主程序
if __name__ == "__main__":
    # 文件路径
    file_path = "/home/JSDC/017254/code/gitee/map_learing/map_grid/03_xyzrpy/rpy/data2/save_lidar.csv"  # 替换为你的文件路径

    lidar_data = load_lidar_data_parse_rpy(file_path)

    # 打印前 5 帧数据
    for i, frame in enumerate(lidar_data[:5]):  # 只打印前 5 帧
        print(f"Frame {i}:")
        print(f"  Time Stamp: {frame['time_stamp']}")
        print(f"  Fusepose: {frame['fusepose']}")
        print(f"  RPY: {frame['rpy']}")
        print(f"  Points: {frame['points'].shape} points loaded")
        print()

    build_grid_map = BuildGridMap(lidar_data)

    GLOBAL_MAP = build_grid_map.get_global_map()

    LOCAL_SIZE = (100, 100)
    # 视频保存设置
    # 7059
    video_filename = "local_video_map.avi"  # 视频文件名
    fps = 20  # 每秒帧数
    image_size = (1000, 1000)  # 图像大小 (高度, 宽度)
    image_size = (455, 531)  # 图像大小 (高度, 宽度)
    fourcc = cv2.VideoWriter_fourcc(*'XVID')  # 视频编码格式
    video_writer = cv2.VideoWriter(video_filename, fourcc, fps, (image_size[1], image_size[0]))

    LOCAL_UPDATE_MAP = np.full(LOCAL_SIZE, 0.5)  # 初始占用概率为 0.5
    update_map = updateMap(LOCAL_UPDATE_MAP, GLOBAL_MAP)
    for i, frame in enumerate(lidar_data[:200]):  # 只打印前 20
        print(f"index: {i}")
        time_index = frame['time_stamp']
        fusepsoe_xyz = frame['fusepose']
        fusepose_rpy = frame['rpy']
        points = frame['points']
        
        # grid_map = build_local_lidar_map(points, (100,100))
        # img = visualize_line(grid_map, i)
        #          update_local_map(self, points, fusepsoe_xyz, fusepose_rpy, i):
        update_map.update_local_map(points, fusepsoe_xyz, fusepose_rpy, i)
        if i % 1  == 0:
            update_map.visual_index_local_map(i, 0.6)
            pass

        img = update_map.visual_index_video_map(i)
        # LOCAL_UPDATE_MAP = build_local_lidar_update_map(LOCAL_UPDATE_MAP, points, fusepsoe_xyz, fusepose_rpy)
        video_writer.write(img)
    video_writer.release()
    print(f"视频已保存为 {video_filename}")
    # a,b,c,d = update_map.get_map_xyz_rpy()
    
    pass